package ar.vc.trans;

import java.util.ArrayList;

import android.annotation.SuppressLint;
import ar.exceptions.SystemNotSolved;

public class TsaiCalib
{
	// ........................................ ZERO
	private static float ZERO = (float) 1.0E-10;// Valor que será tomado como zero nas comparações
	// ........................................ CARACTERÍSTICAS DO PADRÃO DE CALIBRAÇÃO
	private float Dx;							// Dimensão do quadrado em cm na horizontal
    private float Dy;							// Dimensão do quadrado em cm na vertical
    private int    Nx;							// Qnt de pontos na horizontal
    private int    Ny;							// Qnt de pontos na vertical 
    private int    Nt;          				// Qnt total de pontos
    // ........................................ PONTOS PARA CALIBRAÇÃO
    private ArrayList<float[]> pontos_w = new ArrayList<float[]>(); // Pontos reais
    private ArrayList<float[]> pontos_f = new ArrayList<float[]>(); // Pontos da memória frame
    // ........................................ VARIÁVEIS AUXILIARES
    private float[][] A;
    private float[]   B;
    private float[]   U;
    private float     Ty;
    private float     Tx;
    // ........................................ EXTRÍNSECOS
    private float[]   R = new float[9];		// Rotação
    private float[]   T = new float[3];		// Translação
    // ........................................ INTRÍNSECOS
    private float Sx = 1.0f;					// Pixels/unidade de medida na horizontal (default = 1.0f Pfxel/mm)
    private float Sy = 1.0f;					// Pixels/unidade de medida na vertical   (default = 1.0f Pfxel/mm)
    private float k1 = 0.0f;   				// Fator de distorção (default = 0.0f)
    private float f;							// Distancia focal
    private float Cx;							// Projeção do centro óptico componente X
    private float Cy;							// Projeção do centro óptico componente Y
    // ........................................ PIRÂMIDE DE VISÃO
    private float tv_width;					// Tamanho horizontal da imagem em pixels
    private float tv_height;					// Tamanho vertical da imagem em pixels
    private float near;						// Distância do plano anterior
    private float far;							// Distância do plano posterior
    // ........................................ MATRIZES OPENGL
	float[] ogl_mdlv = new float[16];			// Matriz ModelView	
	float[] ogl_proj = new float[16];			// Matriz Projection
  
	/** Inicializa as variáveis do método de calibração */
    public void ar_init_tsai(float Dx, float Dy, int Nx, int Ny, ArrayList<float[]> pontos_f, int heigth, int width)
    {
        this.Dx = Dx;
        this.Dy = Dy;
        this.Nx = Nx;
        this.Ny = Ny; 
        
        this.Nt = Nx*Ny;
        
        this.pontos_f = pontos_f;
        
        this.ar_set_pontos_w();

        this.Cx = width/2;
        this.Cy = heigth/2;
    }
    
    public void ar_init_tsai(ArrayList<float[]> pontos_f, int heigth, int width)
    {
        this.Nt = pontos_f.size();
        
        this.pontos_f = pontos_f;
        
        ar_load_padrao();

        this.Cx = width/2;
        this.Cy = heigth/2;
    }
    
    private void ar_load_padrao()
    {
    	pontos_w.add(new float[] {0,0})     ;pontos_w.add(new float[] {0f,16.5f});
    	pontos_w.add(new float[] {2.8f,3.1f}) ;pontos_w.add(new float[] {2.8f,13.9f});
    	pontos_w.add(new float[] {12.2f,3.1f});pontos_w.add(new float[] {12.2f,13.9f});
    	pontos_w.add(new float[] {22.4f,0f})  ;pontos_w.add(new float[] {22.4f,16.5f});
    }

    /** Gera os pontos do marcador no referêncial do Mundo */
    private void ar_set_pontos_w()
    {
    	for ( int j = 0; j < Ny; j++ )
    		for ( int i = 0; i < Nx; i++ )
    			pontos_w.add(new float[] {i*Dx,j*Dy});
    }

    /** Gera o sistema de equações A.X = B */
    private void ar_set_A_B()
    {
    	A = new float[Nt][5];
    	B = new float[Nt];
    	
    	for ( int i = 0; i < Nt; i++ )
    	{
    		float[] Pf = pontos_f.get(i);
    		float[] Pw = pontos_w.get(i);
    				
    		A[i][0] =  Pf[1]*Pw[0];
    		A[i][1] =  Pf[1]*Pw[1];
    		A[i][2] =  Pf[1];
    		A[i][3] = -Pf[0]*Pw[0];
    		A[i][4] = -Pf[0]*Pw[1];
    		
    		B[i]    =  Pf[0];
    	}
    }

    /** Calcula U */
    private void ar_calculate_U()
    {
    	U = MinimosQuadrados.Solve(A, B);
    }
    
    /** Transforma os pontos do sistema de coordenadas da memória frame (SCP) para o sistema de Coordenadas da Imagem (SCI) 
     *  [SCP] = pixel
     *  [SCI] = mm 
     */
    private void ar_SCP_to_SCI()
    {
    	for ( int i = 0; i < pontos_f.size(); i++ )
    		pontos_f.set(i, new float[] {(pontos_f.get(i)[0] - Cx)/Sx, (pontos_f.get(i)[1] - Cy)/Sy});
    }
    
    /** Distorção - Coordenadas dos pontos da imagem desconsiderando a distorção */
    private void ar_Xd_to_Xu()
    {
    	float ri;
    	for ( int i = 0; i < pontos_f.size(); i++ )
    	{
    		ri  = (float) (Math.pow(pontos_f.get(i)[0],2) + Math.pow(pontos_f.get(i)[1],2));
    		pontos_f.set(i,new float[] {pontos_f.get(i)[0]*(1+(k1*ri)),pontos_f.get(i)[1]*(1+(k1*ri))}); 
    	}
    }

    /** Calcula as componentes Tx e Ty */
    @SuppressLint("FloatMath")
	private void ar_calculate_Tx_Ty()
    {
    	float u1 = U[0];
    	float u2 = U[1];
    	float u3 = U[2];
    	float u4 = U[3];
    	float u5 = U[4];
    	
    	float D  = ((u1*u5) - (u2*u4));
    	float Sr = (float) (Math.pow(u1, 2) + Math.pow(u2, 2) + Math.pow(u4, 2) + Math.pow(u5, 2));
    	
    	if ( Math.abs(D) < ZERO )
    	{
    		Ty = (float) Math.sqrt(1.0/Sr);
    	}
    	else
    	{
    		Ty = (float) (Math.sqrt(Sr - Math.sqrt(Math.pow(Sr, 2) - (4*Math.pow(D,2))))/(2*D));
    	}
    	
    	int f = ar_get_farpoint();
    	
    	R[0] = u1*Ty;
    	R[1] = u2*Ty;
    	Tx   = u3*Ty;
    	R[3] = u4*Ty;
    	R[4] = u5*Ty;
    	
    	float sig_xw = Math.signum( R[0]*pontos_w.get(f)[0] + R[1]*pontos_w.get(f)[1] + Tx );
    	float sig_yw = Math.signum( R[3]*pontos_w.get(f)[0] + R[4]*pontos_w.get(f)[1] + Ty );
    	
    	float sig_xf = Math.signum( pontos_f.get(f)[0] );
		float sig_yf = Math.signum( pontos_f.get(f)[1] );
		
		if ( ( sig_xw*sig_xf < +0.0 ) || ( sig_yw*sig_yf < +0.0 ) )
		{
			Ty *= -1;
		}
		
		T[0] = u2*Ty;
		T[1] = Ty;
    }
    
    /** Retorna o ponto da imagem mais afastado do centro da imagem dentre os pontos fornecidos */
    private int ar_get_farpoint()
    {
    	float f_dist  = 0;
    	float dist    = 0;
    	int    f_point = 0;
    	
    	for (int i = 0 ; i < Nt ; i++ )
    	{
    		float[] Pf = pontos_f.get(i);
    		if ( ( dist = (float) (Math.pow(Pf[0],2) + Math.pow(Pf[1],2)) ) > f_dist )
    		{
	    		f_point = i;
	    		f_dist = dist;
    	    }
    	}
    	return f_point;
    }

    /** Calcula a Matriz de Rotação */
    @SuppressLint("FloatMath")
	private void ar_calculate_R()
    {
    	R[0] = U[0]*T[1];
		R[1] = U[1]*T[1];
		R[2] = (float) Math.sqrt( 1.0f - Math.pow(R[0],2) - Math.pow(R[1],2) );
		
		R[3] = U[3] * T[1];
		R[4] = U[4] * T[1];
		R[5] = (float) Math.sqrt( 1.0f - Math.pow(R[3],2) - Math.pow(R[4],2) );
		
		if ( Math.signum( R[0]*R[3] + R[1]*R[4] ) >= +0.0 )
		{
			R[5] *= -1;
		}
		
		R[6] = R[1]*R[5] - R[2]*R[4];
		R[7] = R[2]*R[3] - R[0]*R[5];
		R[8] = R[0]*R[4] - R[1]*R[3];
    }
    
    /** Calcula o foco e a componente Tz do vetor de Translação */
    private void ar_calculate_f_Tz()
    {
    	A = new float[2*Nt][5];
    	B = new float[2*Nt];
    	
    	float[] Pw;
    	float[] Pf;
    			
    	for ( int i = 0 ; i < Nt ; i++ )
    	{
    		Pw = pontos_w.get(i);
    		Pf = pontos_f.get(i);
    		
    		A[i][0]    =  R[0]*Pw[0] + R[1]*Pw[1] + T[0];
    		A[i][1]    = -Pf[0];
    		B[i]       = (R[6]*Pw[0] + R[7]*Pw[1])*Pf[0];
    		A[i+Nt][0] =  R[3]*Pw[0] + R[4]*Pw[1] + T[1];
    		A[i+Nt][1] = -Pf[1];
    		B[i+Nt]    = (R[6]*Pw[0] + R[7]*Pw[1])*Pf[1];
    	}
    	
		B    = MinimosQuadrados.Solve(A,B);
		f    = B[0];
		T[2] = B[1]; 
    }
    
    /** Calcula a componente Tz do vetor de Translação */
    private void ar_calculate_Tz()
    {
    	A = new float[2*Nt][5];
    	B = new float[2*Nt];
    	
    	float[] Pw;
    	float[] Pf;
    			
    	for ( int i = 0 ; i < Nt ; i++ )
    	{
    		Pw = pontos_w.get(i);
    		Pf = pontos_f.get(i);
    		
    		A[i][0]    = -Pf[0];
    		B[i]       = (R[6]*Pw[0] + R[7]*Pw[1])*Pf[0] - f*(R[0]*Pw[0] + R[1]*Pw[1] + T[0]);
    		A[i+Nt][0] = -Pf[1];
    		B[i+Nt]    = (R[6]*Pw[0] + R[7]*Pw[1])*Pf[1] - f*(R[3]*Pw[0] + R[4]*Pw[1] + T[1]);
    	}
    	
    	T[2] = MinimosQuadrados.Solve(A,B)[0];
    }
     
    /** Ajusta a matriz Projection no padrão da OpenGL */
    private void ar_opengl_projection()
    {
    	float t, b, l, r;
    	
    	t = 2.0f*near*tv_height/2.0f/f;
    	b = -t;
    	r = 2.0f*near*tv_width/2.0f/f;
    	l = -r;
    	
    	ogl_proj[0] = 2*near/(r-l); ogl_proj[4] = 		   0.0f; ogl_proj[ 8] = 	 	      (r + l)/(r - l); ogl_proj[12] = 					 0.0f;
		ogl_proj[1] = 		   0.0f; ogl_proj[5] = 2*near/(t-b); ogl_proj[ 9] = 	 		  (t + b)/(t - b); ogl_proj[13] = 					 0.0f;
		ogl_proj[2] = 		   0.0f; ogl_proj[6] = 	       0.0f; ogl_proj[10] = -(far + near)/(far - near)   ; ogl_proj[14] = -far*near/(far - near);
		ogl_proj[3] = 		   0.0f; ogl_proj[7] = 		   0.0f; ogl_proj[11] = 						-1.0f; ogl_proj[15] = 					 0.0f;
    }
    
    /** Ajusta a matriz ModelView no padrão OpenGL */
    private void ar_opengl_modelview()
    {
    	ogl_mdlv[ 0] = R[0]; ogl_mdlv[ 1] = R[1]; ogl_mdlv[ 2] =-R[2]; ogl_mdlv[ 3] = T[0];
    	ogl_mdlv[ 4] = R[3]; ogl_mdlv[ 5] = R[4]; ogl_mdlv[ 6] =-R[5]; ogl_mdlv[ 7] = T[1];
    	ogl_mdlv[ 8] =-R[6]; ogl_mdlv[ 9] =-R[7]; ogl_mdlv[10] = R[8]; ogl_mdlv[11] = T[2];
    	ogl_mdlv[12] = 0.0f; ogl_mdlv[13] = 0.0f; ogl_mdlv[14] = 0.0f; ogl_mdlv[15] = 1.0f;
    }
    
    /** Executa o método de Calibração */
    public void ar_tsai()
    {
    	try
    	{
    		ar_set_A_B();
    		ar_calculate_U();
    		ar_calculate_Tx_Ty();
    		ar_calculate_R();
    		
    		try
    		{
    			ar_calculate_f_Tz();
    			
    			if ( f < +0.0 )
    			{
    				R[2] *= -1.0;
    				R[5] *= -1.0;
    				R[6] *= -1.0;
    				R[7] *= -1.0;
    				
    				ar_calculate_f_Tz();
    			}
    			
    			ar_opengl_projection();
    			ar_opengl_modelview();
    		}
    		catch( Exception e )
    		{
    			System.out.println("Erro 1");
    		}
    	}
    	catch( Exception e )
		{
			System.out.println("Erro 2");
		}
    }
    
    /** Retorna a matriz Projection do OpenGL */
    public float[] ar_get_ogl_projection()
    {
    	return ogl_proj;
    }
    
    /** Retorna a matriz ModelView do OpenGL */
    public float[] ar_get_ogl_modelview()
    {
    	ogl_mdlv = new float[] {1.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,1.0f};
    	return ogl_mdlv;
    }
}